#include "trace.h"


void GetJumpLocation(uint16_t* jumpLocation, pos p) {
	int byte;
	int location = 0;
	int temp;
	int i;
	if(p == f) {
		for(int i = 0; i < T_FRONT_NUM; i++) {
			if(i == 0) {
				byte = HAL_GPIO_ReadPin(T_FRONT_0_GPIO_PORT, T_FRONT_0_GPIO_PIN);
			} else if(i == 1) {
				byte = HAL_GPIO_ReadPin(T_FRONT_1_GPIO_PORT, T_FRONT_1_GPIO_PIN);
			} else if(i == 2) {
				byte = HAL_GPIO_ReadPin(T_FRONT_2_GPIO_PORT, T_FRONT_2_GPIO_PIN);
			} else if(i == 3) {
				byte = HAL_GPIO_ReadPin(T_FRONT_3_GPIO_PORT, T_FRONT_3_GPIO_PIN);
			} else if(i == 4) {
				byte = HAL_GPIO_ReadPin(T_FRONT_4_GPIO_PORT, T_FRONT_4_GPIO_PIN);
			} else if(i == 5) {
				byte = HAL_GPIO_ReadPin(T_FRONT_5_GPIO_PORT, T_FRONT_5_GPIO_PIN);
			} else if(i == 6) {
				byte = HAL_GPIO_ReadPin(T_FRONT_6_GPIO_PORT, T_FRONT_6_GPIO_PIN);
			}
			location = ByteAdd(location, byte << i);
		}
		for(i = 0; i < T_FRONT_NUM; i++) {
			temp = (location >> i) & 0b0000001;
			if(temp) {
				jumpLocation[0] = i;
				break;
			}
		}
		for(i = 0; i < T_FRONT_NUM; i++) {
			temp = (location << i) & 0b1000000;
			if(temp) {
				jumpLocation[1] = T_FRONT_NUM - 1 - i;
				break;
			}
		}
	} else if(p == b) {
		jumpLocation[0] = HAL_GPIO_ReadPin(T_BEHIND_0_GPIO_PORT, T_BEHIND_0_GPIO_PIN);
		jumpLocation[1] = HAL_GPIO_ReadPin(T_BEHIND_1_GPIO_PORT, T_BEHIND_1_GPIO_PIN);
	} else if(p == l) {
		jumpLocation[0] = HAL_GPIO_ReadPin(T_LEFT_0_GPIO_PORT, T_LEFT_0_GPIO_PIN);
		jumpLocation[1] = HAL_GPIO_ReadPin(T_LEFT_1_GPIO_PORT, T_LEFT_1_GPIO_PIN);
	} else if(p == r) {
		jumpLocation[0] = HAL_GPIO_ReadPin(T_RIGHT_0_GPIO_PORT, T_RIGHT_0_GPIO_PIN);
		jumpLocation[1] = HAL_GPIO_ReadPin(T_RIGHT_1_GPIO_PORT, T_RIGHT_1_GPIO_PIN);
	}
}


int ByteAdd(int a, int b) {
    int re = a;
    while(b){
        int tmp = a;
        a = a^b;
        b = (tmp&b)<<1;
        re = a;
    }
    return re;
}

